
clear all;
close all;

TF = GSLpp_testfunctions();
TFI = { 12, 13 };

PLOT_COND_NUM = true;

TF{12}.H_min = [ 802,   -400,   0,           0.00000;
                -400,    200,   0,           0.00000;
                 0,      0,     802.00000,  -400.00000;
                 0,      0,    -400.00000,   200.00000 ];

TF{13}.H_min = [ 2,   0,    0,   0;
                 20,  200,  0,   0;
                 0,   0,    10,  0;
                 0,   0,   -10,  10 ];

for tfi = 1:length(TFI)
  tf = TF{TFI{tfi}};
  tf.n = 4;

  tf_params = GSLpp_get_testfunction_params(tf);
  x0 = GSLpp_get_startingpoint(tf.starting_point, tf.n);
  xmin = GSLpp_get_minimizer(tf.minimizer, tf.n);

  results = cell(2, 1);

  results{1} = GSLpp_minimize("gsl_ext_bfgs_hess_mt", ...
                              struct, struct, ...
                              tf.name, tf_params, ...
                              struct("f", "sym", "g", "sym", "H", "sym"), ...
                              "xdiff", struct("eps", 1e-14, "xmin", xmin), ...
                              x0, 0, false);

  results{2} = GSLpp_minimize("gsl_ext_bfgs_mt", ...
                              struct, struct, ...
                              tf.name, tf_params, ...
                              struct("f", "sym", "g", "sym", "H", "sym"), ...
                              "xdiff", struct("eps", 1e-14, "xmin", xmin), ...
                              x0, 0, false);

  matrix_diff_norms = cell(2, 1);
  matrix_diff_norms{1} = zeros(results{1}.numiter, 1);
  matrix_diff_norms{2} = zeros(results{2}.numiter, 1);
  dist_to_min = cell(2, 1);
  dist_to_min{1} = zeros(results{1}.numiter, 1);
  dist_to_min{2} = zeros(results{2}.numiter, 1);
  cond_num = cell(2, 1);
  cond_num{1} = zeros(results{1}.numiter, 1);
  cond_num{2} = zeros(results{2}.numiter, 1);
  dm_norms = cell(2, 1);
  dm_norms{1} = zeros(results{1}.numiter, 1);
  dm_norms{2} = zeros(results{2}.numiter, 1);

  for i = 1:2
    for j = 1:results{i}.numiter
      H = results{i}.iterations{j}.H;
      H = tril(H, 0) + tril(H, -1)';

      if i == 1
        B = tril(results{1}.iterations{j}.B, 0) + tril(results{1}.iterations{j}.B, -1)';
        matrix_diff_norms{1}(j) = norm(B - H, "fro")^2;
      else
        S = tril(results{2}.iterations{j}.S, 0) + tril(results{2}.iterations{j}.S, -1)';
        matrix_diff_norms{2}(j) = norm(S - inv(H), "fro")^2;
      endif

      dist_to_min{i}(j) = norm(results{i}.iterations{j}.x - xmin);
      cond_num{i}(j) = cond(H);

      if isfield(tf, "H_min") == true && j < results{i}.numiter
        xdiff = results{i}.iterations{j + 1}.x - results{i}.iterations{j}.x;
        alpha = results{i}.iterations{j + 1}.alpha;
        if i == 1
          dm_norms{1}(j) = norm((B / alpha - tf.H_min) * xdiff) / norm(xdiff);
        else
          dm_norms{2}(j) = norm((inv(S) / alpha - tf.H_min) * xdiff) / norm(xdiff);
        endif
      endif
    endfor
  endfor

  dm_norms{1}(results{1}.numiter) = dm_norms{1}(results{1}.numiter - 1);
  dm_norms{2}(results{2}.numiter) = dm_norms{2}(results{2}.numiter - 1);

  for i = 1:2
    GSLpp_newplot();

    hold on;
    semilogy(1:results{i}.numiter, dm_norms{i}, "-r", "linewidth", 6);
    semilogy(1:results{i}.numiter, matrix_diff_norms{i}, "-g", "linewidth", 6);
    semilogy(1:results{i}.numiter, dist_to_min{i}, "-b", "linewidth", 6);
    if PLOT_COND_NUM == true
      semilogy(1:results{i}.numiter, cond_num{i}, "-c", "linewidth", 6);
    endif
    hold off;

    if i == 1
      approx_norm_str = "||B-H||";
      title("Hessian approximation");
      t_str = "direct";
    else
      approx_norm_str = "||S-H^{-1}||";
      title("Inverse Hessian approximation");
      t_str = "inverse";
    endif
    if PLOT_COND_NUM == false
      legend('\Delta', approx_norm_str, "||x-x*||", "location", "southwest");
    else
      legend('\Delta', approx_norm_str, "||x-x*||", '\kappa(H)', "location", "southwest");
    endif
    legend('boxon');

    print([ "quasinewton_test_", tf.name, "_" , t_str, ".ps" ], "-dashed", "-color", "-landscape", "-FArial:22");
    close;
  endfor
endfor
